{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 操作臂基本运动控制\n",
    "\n",
    "## 硬件准备\n",
    "\n",
    "材料：\n",
    "\n",
    "五自由度操作臂：F功能模块底座、I模块*2、T模块*3、G模块*1\n",
    "\n",
    "六自由度操作臂：F功能模块底座、I模块*3、T模块*3、G模块*1\n",
    "\n",
    "现在五自由度操作臂为例：\n",
    "\n",
    "## 硬件连接\n",
    "\n",
    "连接五自由度操作臂\n",
    "\n",
    "![](img/img_structure.png)\n",
    "\n",
    "\n",
    "## 传感器连接\n",
    "\n",
    "连接五自由度操作臂\n",
    "\n",
    "![](img/img_sensors.png)\n",
    "\n",
    "\n",
    "## 关节配置\n",
    "\n",
    "将硬件连接的关节ID次序更新到配置文件中\n",
    "\n",
    "![](img/img_ids.png)\n",
    "\n",
    "配置完后启动设备\n",
    "\n",
    "\n",
    "# 代码说明\n",
    "\n",
    "### 导入库"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#!/usr/bin/python\n",
    "# -*- coding: UTF-8 -*-\n",
    "import sys;\n",
    "sys.path.append(\"..\")\n",
    "sys.path.append(\"../../\")\n",
    "from lib.BwRobotLib import BwRobotLib\n",
    "from lib.MiniBotControlLib import MiniBotControlLib\n",
    "from RobotManipulator.ManipulatorRobot import ManipulatorRobot\n",
    "from RobotManipulator.ManipulatorActions import ManipulatorActions\n",
    "import keyboard\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 声明库对象"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "#声明库对象\n",
    "robotlib = BwRobotLib()\n",
    "manipulator = ManipulatorRobot(robotlib)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 连接设备"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "连接成功\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#连接机器人设备\n",
    "robotlib.connectRobot(\"192.168.1.107\",55000)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 配置并初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "读取ID配置文件:\n",
      "#操作臂\n",
      "\n",
      "UP:20,30,31,21,32,40\n",
      "\n",
      "#双足-双轮\n",
      "\n",
      "#UP:30,31,20\n",
      "\n",
      "#Down:32,33,21\n",
      "\n",
      "关节模块20位置：-180\n",
      "关节模块30位置：-90\n",
      "关节模块31位置：-89\n",
      "关节模块21位置：-180\n",
      "关节模块32位置：-90\n",
      "关节模块40位置：0\n"
     ]
    }
   ],
   "source": [
    "#加载配置（兼容VS项目启动方式及文件直接启动方式）\n",
    "import os,inspect\n",
    "fileName = inspect.getframeinfo(inspect.currentframe()).filename\n",
    "folderPath     = os.path.dirname(os.path.abspath(fileName))\n",
    "cfile=folderPath+\"/config.txt\"\n",
    "robotlib.loadConfig(cfile)\n",
    "\n",
    "#配置操作臂\n",
    "manipulator.bindIds(robotlib.module_ids)\n",
    "\n",
    "#读取当前坐标以初始化位置信息\n",
    "manipulator.readAngles()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 控制空间坐标处理方法"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "#控制空间坐标\n",
    "def controlCoordinate(key):\n",
    "    cmd = key.name\n",
    "    #print(cmd)\n",
    "    #X\n",
    "    if(cmd == 'i'):\n",
    "        val = manipulator.spatial_coordinate[0] + 1\n",
    "        manipulator.spatial_coordinate[0] = val\n",
    "    if(cmd == 'k'):\n",
    "        val = manipulator.spatial_coordinate[0] - 1\n",
    "        manipulator.spatial_coordinate[0] = val\n",
    "\n",
    "    #Y\n",
    "    if(cmd == 'o'):\n",
    "        val = manipulator.spatial_coordinate[1] + 1\n",
    "        manipulator.spatial_coordinate[1] = val\n",
    "    if(cmd == 'l'):\n",
    "        val = manipulator.spatial_coordinate[1] - 1\n",
    "        manipulator.spatial_coordinate[1] = val\n",
    "\n",
    "    #Z\n",
    "    if(cmd == 'p'):\n",
    "        val = manipulator.spatial_coordinate[2] + 1\n",
    "        manipulator.spatial_coordinate[2] = val\n",
    "    if(cmd == 'm'):\n",
    "        val = manipulator.spatial_coordinate[2] - 1\n",
    "        manipulator.spatial_coordinate[2] = val\n",
    "\n",
    "    #Roll\n",
    "    if(cmd == 'z'):\n",
    "        val = manipulator.spatial_coordinate[3] + 1\n",
    "        manipulator.spatial_coordinate[3] = val\n",
    "    if(cmd == 'x'):\n",
    "        val = manipulator.spatial_coordinate[3] - 1\n",
    "        manipulator.spatial_coordinate[3] = val\n",
    "\n",
    "    #Pitch\n",
    "    if(cmd == 'c'):\n",
    "        val = manipulator.spatial_coordinate[4] + 1\n",
    "        manipulator.spatial_coordinate[4] = val\n",
    "    if(cmd == 'v'):\n",
    "        val = manipulator.spatial_coordinate[4] - 1\n",
    "        manipulator.spatial_coordinate[4] = val\n",
    "\n",
    "     #Yaw\n",
    "    if(cmd == 'b'):\n",
    "        val = manipulator.spatial_coordinate[5] + 1\n",
    "        manipulator.spatial_coordinate[5] = val\n",
    "    if(cmd == 'n'):\n",
    "        val = manipulator.spatial_coordinate[5] - 1\n",
    "        manipulator.spatial_coordinate[5] = val\n",
    "\n",
    "    manipulator.pushSpatialCoordinate()\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 控制关节角度处理方法"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def controlAngles(key):\n",
    "    cmd = key.name\n",
    "    #print(cmd)\n",
    "    #I\n",
    "    if(cmd == 'q'):\n",
    "        val = manipulator.position_angles[0] + 1\n",
    "        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)\n",
    "    if(cmd == 'a'):\n",
    "        val = manipulator.position_angles[0] - 1\n",
    "        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)\n",
    "\n",
    "    #T\n",
    "    if(cmd == 'w'):\n",
    "        val = manipulator.position_angles[1] + 1\n",
    "        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)\n",
    "    if(cmd == 's'):\n",
    "        val = manipulator.position_angles[1] - 1\n",
    "        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)\n",
    "\n",
    "    #T\n",
    "    if(cmd == 'e'):\n",
    "        val = manipulator.position_angles[2] + 1\n",
    "        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)\n",
    "    if(cmd == 'd'):\n",
    "        val = manipulator.position_angles[2] - 1\n",
    "        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)\n",
    "\n",
    "    #I\n",
    "    if(cmd == 'r'):\n",
    "        val = manipulator.position_angles[3] + 1\n",
    "        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)\n",
    "    if(cmd == 'f'):\n",
    "        val = manipulator.position_angles[3] - 1\n",
    "        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)\n",
    "\n",
    "    #T\n",
    "    if(cmd == 't'):\n",
    "        val = manipulator.position_angles[4] + 1\n",
    "        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)\n",
    "    if(cmd == 'g'):\n",
    "        val = manipulator.position_angles[4] - 1\n",
    "        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)\n",
    "\n",
    "    max = 180\n",
    "    min = -180\n",
    "    if(manipulator.ids_size <= 6):\n",
    "        max = 90\n",
    "        min = 0\n",
    "     #G/I\n",
    "    if(cmd == 'y'):      \n",
    "        val = manipulator.position_angles[5] + 1\n",
    "        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)\n",
    "    if(cmd == 'h'):\n",
    "        val = manipulator.position_angles[5] - 1\n",
    "        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)\n",
    "\n",
    "    #G\n",
    "    if(cmd == 'u'):\n",
    "        if(manipulator.ids_size > 6):        \n",
    "           val = manipulator.position_angles[6] + 1\n",
    "           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)\n",
    "    if(cmd == 'j'):\n",
    "        if(manipulator.ids_size > 6):   \n",
    "           val = manipulator.position_angles[6] - 1\n",
    "           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)\n",
    "\n",
    "    manipulator.pushJointsAngles()\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 捕捉键盘值"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "#处理控制键\n",
    "def handleControlKeys(key):\n",
    "     cmd = key.name\n",
    "     #退出监测\n",
    "     if(cmd == '0'):\n",
    "        #keyboard.unhook_all()\n",
    "        return\n",
    "\n",
    "     if(cmd == 'i' or cmd == 'k' or cmd == 'o' or cmd == 'l' or cmd == 'p' or cmd == 'm' or cmd == 'z' or cmd == 'x' or cmd == 'c' or cmd == 'v' or cmd == 'b' or cmd == 'n'):\n",
    "        controlCoordinate(key) \n",
    "\n",
    "     if(cmd == 'q' or cmd == 'a' or cmd == 'w' or cmd == 's' or cmd == 'e' or cmd == 'd' or cmd == 'r' or cmd == 'f' or cmd == 't' or cmd == 'g' or cmd == 'y' or cmd == 'h' or cmd == 'u' or cmd == 'j'):\n",
    "        controlAngles(key) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 创建动作组管理对象"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "#动作管理\n",
    "manipulatorActions = ManipulatorActions(robotlib)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 动作组命令处理"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "#动作组命令\n",
    "def handleActionsCmd(key):\n",
    "    cmd = key.name\n",
    "    #添加当前姿态到动作列表\n",
    "    if(cmd == '1'):\n",
    "        if(manipulatorActions.is_motorsEnable is False):\n",
    "            manipulatorActions.readMotorsPositions(manipulator)\n",
    "        manipulatorActions.add(manipulator.position_angles)\n",
    "\n",
    "    #运行动作列表\n",
    "    if(cmd == '2'):\n",
    "        manipulatorActions.run()\n",
    "\n",
    "    #停止动作\n",
    "    if(cmd == '3'):\n",
    "        manipulatorActions.stop()\n",
    "\n",
    "    #清空当前动作列表\n",
    "    if(cmd == '4'):\n",
    "        manipulatorActions.clear()\n",
    "\n",
    "    #关闭模块使能\n",
    "    if(cmd == '5'):\n",
    "        manipulatorActions.enableMotors(False)\n",
    "\n",
    "    #打开模块使能\n",
    "    if(cmd == '6'):\n",
    "        manipulatorActions.enableMotors(True)\n",
    "    pass\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 动作组及动作命令分发"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "def handleActionControlCmd(key):\n",
    "    isDown = key.event_type == 'down'\n",
    "    if not isDown:\n",
    "        return\n",
    "\n",
    "    cmd = key.name\n",
    "    #添加当前姿态到动作列表\n",
    "    if(cmd == '1' or cmd == '2' or cmd == '3' or cmd == '4' or cmd == '5' or cmd == '6'):\n",
    "        #if(had_handled_cmd!=cmd):\n",
    "        #   handleActionsCmd(key)\n",
    "        handleActionsCmd(key)\n",
    "    else:\n",
    "        handleControlKeys(key)\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 动作及动作组命令提示"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "#新增动作组\n",
    "def controlActionsTips():\n",
    "    print(\"Keyboard to control angle: Increase / Reduce\")\n",
    "    print(\"Joint 1 : Q/A;\")\n",
    "    print(\"Joint 2 : W/S;\")\n",
    "    print(\"Joint 3 : E/D;\")\n",
    "    print(\"Joint 4 : R/F;\")\n",
    "    print(\"Joint 5 : T/G;\")\n",
    "    print(\"Joint 6 : Y/H;\")\n",
    "    print(\"Joint 7 : U/J;\")\n",
    "\n",
    "    print(\"Coordinate X : I/K;\")\n",
    "    print(\"Coordinate Y : O/L;\")\n",
    "    print(\"Coordinate Z : P/M;\")\n",
    "    print(\"Coordinate Roll: Z/X;\")\n",
    "    print(\"Coordinate Pitch: C/V;\")\n",
    "    print(\"Coordinate Yaw: B/N\")\n",
    "\n",
    "    print(\"\")\n",
    "    print(\"命令:\")\n",
    "    print(\"1 :新增姿态\")\n",
    "    print(\"2 :运行\")\n",
    "    print(\"3 :停止\")\n",
    "    print(\"4 :清空当前动作组\")\n",
    "    print(\"5 :关闭构型模块使能\")\n",
    "    print(\"6 :开启构型模块使能\")\n",
    "    print(\"0 :退出\")\n",
    "    #print(\"-----------------------\")\n",
    "    #print(\"0 :Exit\")\n",
    "    keyboard.hook(handleActionControlCmd)\n",
    "    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce\n",
    "    keyboard.wait('0') # keyboard.wait() 的作用是阻塞进程，直到按下 Ctrl 键。\n",
    "    keyboard.unhook_all()\n",
    "    input(\"输入Enter键继续.....\")\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 传感器遥控动作组处理"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "#遥控动作组\n",
    "def controlActionsSensorControlTips():\n",
    "    count = len(manipulatorActions.actions_groups)\n",
    "    print(\"所有动作组：\" + str(count) + \"个\")\n",
    "\n",
    "    print(\"请按红外遥控以控制执行动作组...\")\n",
    "    print(\"(遥控 1.表示执行动作组一；2.表示执行动作组二.....)\")\n",
    "    print(\"遥控开关 :退出\")\n",
    "\n",
    "    while True:\n",
    "        cmd = robotlib.getYaokongSensorCmd()\n",
    "        if(cmd == 0x13):\n",
    "            break\n",
    "        if(cmd > 0):\n",
    "            num = cmd - 0xA + 1\n",
    "            b = manipulatorActions.loadGroup(num)\n",
    "            if b:\n",
    "               manipulatorActions.run()\n",
    "        time.sleep(0.01)\n",
    "        pass\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 主入口命令提示"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "#主入口命令提示\n",
    "def mainIndexTips():     \n",
    "    while True:\n",
    "      count = len(manipulatorActions.actions_groups)\n",
    "      print(\"所有动作组：\" + str(count) + \"个\")\n",
    "      print(\"请输入序号以编辑（如：1）\")    \n",
    "      print(\"-----------------------\")\n",
    "      print(\"其它命令:\")\n",
    "      print(\"N :新增动作组\")\n",
    "      print(\"C :遥控动作组\")\n",
    "      print(\"E :退出\")\n",
    "      \n",
    "      cmd = input(\"请输入：\")\n",
    "      if(cmd == 'N' or cmd == 'n'):\n",
    "         manipulatorActions.newGroup()\n",
    "         #count=len(manipulatorActions.actions_groups)\n",
    "         #print(\"所有动作组：\"+str(count)+\"个\")\n",
    "         controlActionsTips()\n",
    "\n",
    "      elif(cmd == 'C' or cmd == 'c'):\n",
    "         controlActionsSensorControlTips()\n",
    "      elif(cmd == 'E' or cmd == 'e'):\n",
    "         break\n",
    "      else:\n",
    "         print(\"尝试打开序号为：\" + cmd + \"的动作组\")\n",
    "         num = robotlib.converToNum(cmd)\n",
    "         manipulatorActions.loadGroup(num)\n",
    "         controlActionsTips()\n",
    "    pass"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 程序开始"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "所有动作组：0个\n",
      "请输入序号以编辑（如：1）\n",
      "-----------------------\n",
      "其它命令:\n",
      "N :新增动作组\n",
      "C :遥控动作组\n",
      "E :退出\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "请输入： n\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Keyboard to control angle: Increase / Reduce\n",
      "Joint 1 : Q/A;\n",
      "Joint 2 : W/S;\n",
      "Joint 3 : E/D;\n",
      "Joint 4 : R/F;\n",
      "Joint 5 : T/G;\n",
      "Joint 6 : Y/H;\n",
      "Joint 7 : U/J;\n",
      "Coordinate X : I/K;\n",
      "Coordinate Y : O/L;\n",
      "Coordinate Z : P/M;\n",
      "Coordinate Roll: Z/X;\n",
      "Coordinate Pitch: C/V;\n",
      "Coordinate Yaw: B/N\n",
      "\n",
      "命令:\n",
      "1 :新增姿态\n",
      "2 :运行\n",
      "3 :停止\n",
      "4 :清空当前动作组\n",
      "5 :关闭构型模块使能\n",
      "6 :开启构型模块使能\n",
      "0 :退出\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "输入Enter键继续..... 0\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "所有动作组：1个\n",
      "请输入序号以编辑（如：1）\n",
      "-----------------------\n",
      "其它命令:\n",
      "N :新增动作组\n",
      "C :遥控动作组\n",
      "E :退出\n"
     ]
    },
    {
     "name": "stdin",
     "output_type": "stream",
     "text": [
      "请输入： c\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "所有动作组：1个\n",
      "请按红外遥控以控制执行动作组...\n",
      "(遥控 1.表示执行动作组一；2.表示执行动作组二.....)\n",
      "遥控开关 :退出\n"
     ]
    }
   ],
   "source": [
    "mainIndexTips()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
